// label  balck green white  1 2  3
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void saveCalib(string& out_file)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"error"<<endl;         

    outfile<<"P0: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P1: "<<0<<" "<<0<<" "<<0<<" "<<0<<" "
    <<0<<" "<<0<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<0<<" "<<0<<std::endl;
    outfile<<"P2: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
    <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
    outfile<<"P3: "<<319.9988245765257<<" "<<0<<" "<<320.5<<" "<<0<<" "
    <<0<<" "<<319.9988245765257<<" "<<240.5<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<0<<std::endl;
    outfile<<"R0_rect: "<<1<<" "<<0<<" "<<0<<" "
    <<0<<" "<<1<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<std::endl;
    outfile<<"Tr_velo_to_cam: "<<0<<" "<<-1<<" "<<0<<" "<<0.0<<" "
    <<0<<" "<<0<<" "<<-1<<" "<<-0.133<<" "
     <<1<<" "<<0<<" "<<0<<" "<<-0.23<<std::endl;
    outfile<<"Tr_imu_to_velo: "<<1<<" "<<0<<" "<<0<<" "<<0.23<<" "
    <<0<<" "<<1<<" "<<0<<" "<<0<<" "
     <<0<<" "<<0<<" "<<1<<" "<<-0.133<<std::endl;

    outfile.close();
}

vector<int> project2image2dbbox(double h, double w, double l, double x, double y, double z, double yaw){
    MatrixXd xyz(3,8);
    xyz<<l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.,
            0,0,0,0,-h,-h,-h,-h,
            w / 2., -w / 2., -w / 2., w / 2., w / 2., -w / 2., -w / 2., w / 2.;
//    cout<<"hwl"<<h<<" "<<w<<" "<<l<<endl;
//    xyz<<-w/2, -w/2,w/2,w/2,-w/2, -w/2,w/2,w/2,
//            -h,0,0,-h,-h,0,0,-h,
//            l/2,l/2,l/2,l/2,-l/2,-l/2,-l/2,-l/2;

    MatrixXd rotM (3,3);
    rotM<<cos(yaw), 0,sin(yaw),
            0, 1,0,
            -sin(yaw), 0, cos(yaw);
//    rotM=rotM.transpose();
    xyz = rotM*xyz;
    xyz(0,0) = xyz(0,0) + x;xyz(0,1) = xyz(0,1)  + x; xyz(0,2) = xyz(0,2) + x; xyz(0,3) = xyz(0,3) + x;
    xyz(0,4) = xyz(0,4) + x;xyz(0,5) = xyz(0,5) + x;xyz(0,6) = xyz(0,6) + x;xyz(0,7) = xyz(0,7) + x;
    xyz(1,0) = xyz(1,0) + y; xyz(1,1) = xyz(1,1) + y; xyz(1,2) = xyz(1,2) + y; xyz(1,3) = xyz(1,3) + y;
    xyz(1,4) = xyz(1,4) + y;xyz(1,5) = xyz(1,5) + y;xyz(1,6) = xyz(1,6) + y;xyz(1,7) = xyz(1,7) + y;
    xyz(2,0) = xyz(2,0) + z; xyz(2,1) = xyz(2,1) + z; xyz(2,2) = xyz(2,2) + z; xyz(2,3) = xyz(2,3) + z;
    xyz(2,4) = xyz(2,4) + z;xyz(2,5) = xyz(2,5) + z;xyz(2,6) = xyz(2,6) + z;xyz(2,7) = xyz(2,7) + z;


    MatrixXd P2 (3,4);
    P2<<319.9988245765257, 0,320.5,0,
            0,319.9988245765257,240.5,0,
            0,0,1,0;

    MatrixXd xyz_new_M (4,8);
    xyz_new_M<<xyz(0,0), xyz(0,1),xyz(0,2),xyz(0,3),xyz(0,4),xyz(0,5),xyz(0,6),xyz(0,7),
            xyz(1,0), xyz(1,1),xyz(1,2),xyz(1,3),xyz(1,4),xyz(1,5),xyz(1,6),xyz(1,7),
            xyz(2,0), xyz(2,1),xyz(2,2),xyz(2,3),xyz(2,4),xyz(2,5),xyz(2,6),xyz(2,7),
            1,1,1,1,1,1,1,1;
//    cout<<"my debug bbox"<<xyz_new_M<<endl;
    xyz=P2*xyz_new_M;

    vector<double> x_image,y_image;
    for(int i=0; i<8; i++){
        x_image.push_back(xyz(0,i)/xyz(2,i));
        y_image.push_back(xyz(1,i)/xyz(2,i));
//        cout<<"x:"<<xyz(0,i)/xyz(2,i)<<endl;
//        cout<<"y:"<<xyz(1,i)/xyz(2,i)<<endl;
    }


    vector<int> bbox;
    bbox.push_back(int(*min_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*min_element(y_image.begin(),y_image.end())));
    bbox.push_back(int(*max_element(x_image.begin(),x_image.end())));
    bbox.push_back(int(*max_element(y_image.begin(),y_image.end())));
    if(bbox[0]<0)
        bbox[0]=0;
    if(bbox[1]<0)
        bbox[1]=0;
    if(bbox[2]>640)
        bbox[2]=640;
    if(bbox[3]>480)
        bbox[3]=480;
//    cout<<"my debug bbox"<<bbox[0]<<" "<<bbox[1]<<" "<<bbox[2]<<" "<<bbox[3]<<endl;
    return bbox;
}

void saveLabel(string& out_file, vector<vector<double>> label)
{
    ofstream outfile;      //终端输入--》内存--》文本
    outfile.open(out_file.c_str());//(输入流) （变量）（输出文件流）
    if(!outfile) cout<<"error"<<endl;         

    int object_num=label.size();
    vector<int> bbox_image;
    for(int i=0; i<object_num; i++){

        bbox_image=project2image2dbbox(label[i][0],label[i][1],label[i][2],label[i][3],label[i][4],label[i][5],label[i][6]);

        outfile<<"Car "<<0<<" "<<0<<" "<<0<<" "
        <<bbox_image[0]<<" "<<bbox_image[1]<<" "<<bbox_image[2]<<" "<<bbox_image[3]<<" "  //2dbbbox
        <<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" " //hwl
        <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<" " //xyz
        <<label[i][6]<<" "<<0<<std::endl;  //yaw
    }
    outfile.close();
}

void saveTrajectory(string& out_file,double time_stamp, double x,double y,double z,double ow,double ox,double oy,double oz, double vx,double vy,double vz)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"error"<<endl;
    outfile<<time_stamp<<" "<<x<<" "<<y<<" "<<z<<" "    //xyz
           <<ow<<" "<<ox<<" "<<oy<<" "<<oz<<" "<<vx<<" "<<vy<<" "<<vz<<std::endl;  //
    outfile.close();
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& front_image_msg,const nav_msgs::OdometryConstPtr& master_traj_gt_msg, const nav_msgs::OdometryConstPtr& black_traj_gt_msg,
             const nav_msgs::OdometryConstPtr& green_traj_gt_msg, const nav_msgs::OdometryConstPtr& white_traj_gt_msg){
    ROS_INFO("cb ok");
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg, *pc_tmp);

    tf::Transform T_mas,T_black,T_green,T_white,T_mas2black,T_mas2green,T_mas2white,T_lidar2base,T_cam2lidar,center_temp,T_obect;
    T_mas.setOrigin(tf::Vector3(master_traj_gt_msg->pose.pose.position.x, master_traj_gt_msg->pose.pose.position.y, master_traj_gt_msg->pose.pose.position.z));
    T_mas.setRotation(tf::Quaternion(master_traj_gt_msg->pose.pose.orientation.x,master_traj_gt_msg->pose.pose.orientation.y,
                                     master_traj_gt_msg->pose.pose.orientation.z,master_traj_gt_msg->pose.pose.orientation.w));
    T_black.setOrigin(tf::Vector3(black_traj_gt_msg->pose.pose.position.x, black_traj_gt_msg->pose.pose.position.y, black_traj_gt_msg->pose.pose.position.z));
    T_black.setRotation(tf::Quaternion(black_traj_gt_msg->pose.pose.orientation.x,black_traj_gt_msg->pose.pose.orientation.y,
                                       black_traj_gt_msg->pose.pose.orientation.z,black_traj_gt_msg->pose.pose.orientation.w));
    T_green.setOrigin(tf::Vector3(green_traj_gt_msg->pose.pose.position.x, green_traj_gt_msg->pose.pose.position.y, green_traj_gt_msg->pose.pose.position.z));
    T_green.setRotation(tf::Quaternion(green_traj_gt_msg->pose.pose.orientation.x,green_traj_gt_msg->pose.pose.orientation.y,
                                       green_traj_gt_msg->pose.pose.orientation.z,green_traj_gt_msg->pose.pose.orientation.w));
    T_white.setOrigin(tf::Vector3(white_traj_gt_msg->pose.pose.position.x, white_traj_gt_msg->pose.pose.position.y, white_traj_gt_msg->pose.pose.position.z));
    T_white.setRotation(tf::Quaternion(white_traj_gt_msg->pose.pose.orientation.x,white_traj_gt_msg->pose.pose.orientation.y,
                                       white_traj_gt_msg->pose.pose.orientation.z,white_traj_gt_msg->pose.pose.orientation.w));

    T_lidar2base.setOrigin(tf::Vector3(0, 0,-0.617)); //is ground or base_link?
    T_lidar2base.setRotation(tf::Quaternion(0,0,0,1));
    // T_cam2lidar.setOrigin(tf::Vector3(0.230, 0,-0.133));
    T_cam2lidar.setOrigin(tf::Vector3(0.230, 0.0,-0.133));
    T_cam2lidar.setRotation(tf::Quaternion(M_PI/2,0,-M_PI/2));
//    cout<<tf::Matrix3x3(T_cam2lidar.getRotation())[0][0]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[0][1]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[0][2]<<" "<<
//    tf::Matrix3x3(T_cam2lidar.getRotation())[1][0]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[1][1]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[1][2]<<" "<<
//    tf::Matrix3x3(T_cam2lidar.getRotation())[2][0]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[2][1]<<" "<<tf::Matrix3x3(T_cam2lidar.getRotation())[2][2]<<" "<<endl;
    T_cam2lidar=T_cam2lidar.inverse();
//    T_obect.setOrigin(tf::Vector3(0, 0,0));
//    T_obect.setRotation(tf::Quaternion(M_PI/2,0,-M_PI/2));
    T_mas2black=T_lidar2base*T_mas.inverse()*T_black;
    T_mas2green=T_lidar2base*T_mas.inverse()*T_green;
    T_mas2white=T_lidar2base*T_mas.inverse()*T_white;

    double hwl[3];
    int point_num_threhold_=0;
    hwl[0]=0.62;hwl[1]=0.8;hwl[2]=1;

    vector<vector<double>> label;
//    cout<<"black points number"<<filtered_body->points.size()<<endl;
    if(1>point_num_threhold_){
        vector<double> label_this(7);
        label_this[0]=hwl[0];label_this[1]=hwl[1];label_this[2]=hwl[2];
//        center_temp=T_cam2lidar*T_mas2black*T_obect;
        center_temp=T_cam2lidar*T_mas2black;

        label_this[3]=center_temp.getOrigin().x();label_this[4]=center_temp.getOrigin().y();label_this[5]=center_temp.getOrigin().z();
        double r,p;
        tf::Matrix3x3(T_mas2black.getRotation()).getRPY(r,p,label_this[6]);
        if(label_this[6]>=0&&label_this[6]<=M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]>M_PI/2&&label_this[6]<=M_PI){
            label_this[6]=label_this[6]-3*M_PI/2;
        }
        else if(label_this[6]<-M_PI/2&&label_this[6]>=-M_PI){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]<0&&label_this[6]>=-M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        label_this[6]=-label_this[6];
        label.push_back(label_this);
    }

    if(1>point_num_threhold_){
        vector<double> label_this(7);
        label_this[0]=hwl[0];label_this[1]=hwl[1];label_this[2]=hwl[2];
//        center_temp=T_cam2lidar*T_mas2green*T_obect;
    //    cout<<"lidar"<<T_cam2lidar.getOrigin().x()<<" "<<T_cam2lidar.getOrigin().y()<<" "<<T_cam2lidar.getOrigin().z()<<endl;
        center_temp=T_cam2lidar*T_mas2green;

        label_this[3]=center_temp.getOrigin().x();label_this[4]=center_temp.getOrigin().y();label_this[5]=center_temp.getOrigin().z();
        double r,p;
        tf::Matrix3x3(T_mas2green.getRotation()).getRPY(r,p,label_this[6]);
        if(label_this[6]>=0&&label_this[6]<=M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]>M_PI/2&&label_this[6]<=M_PI){
            label_this[6]=label_this[6]-3*M_PI/2;
        }
        else if(label_this[6]<-M_PI/2&&label_this[6]>=-M_PI){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]<0&&label_this[6]>=-M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        label_this[6]=-label_this[6];
        label.push_back(label_this);
    }

    if(1>point_num_threhold_){
        vector<double> label_this(7);
        label_this[0]=hwl[0];label_this[1]=hwl[1];label_this[2]=hwl[2];
//        center_temp=T_cam2lidar*T_mas2white*T_obect;
        center_temp=T_cam2lidar*T_mas2white;

        label_this[3]=center_temp.getOrigin().x();label_this[4]=center_temp.getOrigin().y();label_this[5]=center_temp.getOrigin().z();
        double r,p;
        tf::Matrix3x3(T_mas2white.getRotation()).getRPY(r,p,label_this[6]);
        if(label_this[6]>=0&&label_this[6]<=M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]>M_PI/2&&label_this[6]<=M_PI){
            label_this[6]=label_this[6]-3*M_PI/2;
        }
        else if(label_this[6]<-M_PI/2&&label_this[6]>=-M_PI){
            label_this[6]=label_this[6]+M_PI/2;
        }
        else if(label_this[6]<0&&label_this[6]>=-M_PI/2){
            label_this[6]=label_this[6]+M_PI/2;
        }
        label_this[6]=-label_this[6];
        label.push_back(label_this);
    }

//    if(label.size()==0)
//        return;

//    string tranin_or_test="training";
    string tranin_or_test="testing";
    string dataset_name = "06181";
    cout<<tranin_or_test<<endl;
   pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/label_2/"+frame_id_string_+".txt";
   saveLabel(pc_path_name_,label);


   pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".bin";
   pcd2bin(pc_tmp,pc_path_name_);

   cv_bridge::CvImagePtr image_front;
   image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
   cv::imwrite("/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png",(*image_front).image);

   pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/calib/"+frame_id_string_+".txt";
   saveCalib(pc_path_name_);

    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/master.txt";
    saveTrajectory(pc_path_name_, master_traj_gt_msg->header.stamp.toSec(),
            master_traj_gt_msg->pose.pose.position.x,master_traj_gt_msg->pose.pose.position.y,master_traj_gt_msg->pose.pose.position.z,
                                master_traj_gt_msg->pose.pose.orientation.w,master_traj_gt_msg->pose.pose.orientation.x,
                                master_traj_gt_msg->pose.pose.orientation.y,master_traj_gt_msg->pose.pose.orientation.z,
                   master_traj_gt_msg->twist.twist.linear.x,master_traj_gt_msg->twist.twist.linear.y,master_traj_gt_msg->twist.twist.linear.z);

    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/black.txt";
    saveTrajectory(pc_path_name_,black_traj_gt_msg->header.stamp.toSec(),
                   black_traj_gt_msg->pose.pose.position.x,black_traj_gt_msg->pose.pose.position.y,black_traj_gt_msg->pose.pose.position.z,
                   black_traj_gt_msg->pose.pose.orientation.w,black_traj_gt_msg->pose.pose.orientation.x,
                   black_traj_gt_msg->pose.pose.orientation.y,black_traj_gt_msg->pose.pose.orientation.z,
                   black_traj_gt_msg->twist.twist.linear.x,black_traj_gt_msg->twist.twist.linear.y,black_traj_gt_msg->twist.twist.linear.z);

    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/green.txt";
    saveTrajectory(pc_path_name_,green_traj_gt_msg->header.stamp.toSec(),
                   green_traj_gt_msg->pose.pose.position.x,green_traj_gt_msg->pose.pose.position.y,green_traj_gt_msg->pose.pose.position.z,
                   green_traj_gt_msg->pose.pose.orientation.w,green_traj_gt_msg->pose.pose.orientation.x,
                   green_traj_gt_msg->pose.pose.orientation.y,green_traj_gt_msg->pose.pose.orientation.z,
                   green_traj_gt_msg->twist.twist.linear.x,green_traj_gt_msg->twist.twist.linear.y,green_traj_gt_msg->twist.twist.linear.z);


    pc_path_name_="/home/st/ubuntu_data/dataset/gazebo/"+dataset_name+"/"+tranin_or_test+"/trajectory/white.txt";
    saveTrajectory(pc_path_name_,white_traj_gt_msg->header.stamp.toSec(),
                   white_traj_gt_msg->pose.pose.position.x,white_traj_gt_msg->pose.pose.position.y,white_traj_gt_msg->pose.pose.position.z,
                   white_traj_gt_msg->pose.pose.orientation.w,white_traj_gt_msg->pose.pose.orientation.x,
                   white_traj_gt_msg->pose.pose.orientation.y,white_traj_gt_msg->pose.pose.orientation.z,
                   white_traj_gt_msg->twist.twist.linear.x,white_traj_gt_msg->twist.twist.linear.y,white_traj_gt_msg->twist.twist.linear.z);


//    cout<<"pc timestampe:"<<pc_msg->header.stamp.toSec()<<endl;
//    cout<<"image timestampe:"<<front_image_msg->header.stamp.toSec()<<endl;
//    cout<<"master timestampe:"<<master_traj_gt_msg->header.stamp.toSec()<<endl;
//    cout<<"black timestampe:"<<black_traj_gt_msg->header.stamp.toSec()<<endl;
//    cout<<"green timestampe:"<<green_traj_gt_msg->header.stamp.toSec()<<endl;
//    cout<<"white timestampe:"<<white_traj_gt_msg->header.stamp.toSec()<<endl;

    frame_num_++;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
//    tf2_ros::TransformListener tfl_(tfbuf_);
//    frame_ids_["odom"]=std::string("odom");
//    frame_ids_["base_link"]=std::string("base_link");
//    frame_ids_["map"]=std::string("map");
//    cloud_pub=nh.advertise<sensor_msgs::PointCloud2>("cloud_accum_convert",10,true);
//    ros::Subscriber cloud_sub=nh.subscribe("/cloud",10,cloudCB);



    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/robot1/Ruby80", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/robot1/front_camera/image_raw", 1);
    message_filters::Subscriber<nav_msgs::Odometry> master_traj_sub(nh, "/robot1/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> black_traj_sub(nh, "/nexus3/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> green_traj_sub(nh, "/nexus0/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> white_traj_sub(nh, "/nexus2/base_pose_ground_truth", 1);
//    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_left_sub, image_right_sub,pc_sub, 10);
//    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::Image, nav_msgs::Odometry,nav_msgs::Odometry,nav_msgs::Odometry,nav_msgs::Odometry> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, image_front_sub, master_traj_sub,black_traj_sub, green_traj_sub, white_traj_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.05));
    ROS_INFO("ok1");
    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3,_4,_5,_6));
    ROS_INFO("ok2");
    ros::spin();

    return 0;
}
